//#define __FORCE_TRACE__
//#define __FORCE_DEBUG__
//#define __FORCE_INFO__
#define THIS_FILE "tools\fn_getRoadPath.sqf"
#include <tmf_constants.h>
#include <tmf_macros.h>

TRACE_1("Start",_this);
TMFPROFILERSTART;

private ["_path","_deserialized","_start","_end","_max_dist","_distance","_road","_roads","_nearest_road","_nearest_dist","_fnc_recurse_road_path","_give_up","_time_out"];
DEFAULT_PARAM(_start,0,ARR_3(0,0,0));
DEFAULT_PARAM(_end,1,ARR_3(0,0,0));
DEFAULT_PARAM(_max_dist,2,300);
DEFAULT_PARAM(_give_up,3,3000);
DEFAULT_PARAM(_time_out,4,120);

_path = [];
_deserialized = [];
if (_give_up == -1) then { _give_up = 3000 };
if (_time_out == -1) then { _time_out = 120 };
_distance = (_start distance _end);

// diag_log Format["(%1)[%2]: %3 %4",diag_tickTime,"XfGetRoadPath","Start", _this];
_fnc_recurse_road_path = {
	private ["_current_road","_current_path","_current_idx","_return_path","_temp_path","_end_point","_end_distance","_roads","_road_pos","_found_path","_max_distance", "_min_distance", "_give_up","_best_path","_best_count","_road_serialized","_closest","_total_dist","_current_dist","_time_out"];
	_current_road = _this select 0;
	_current_path = _this select 1;
	_end_point = _this select 2;
	_end_distance = _this select 3;
	_max_distance = _this select 4;
	_give_up = _this select 5;
	_total_dist = _this select 6;
	_time_out = _this select 7;
	_current_dist = 0;
	// diag_log Format["(%1)[%2]: %3",diag_tickTime,"_fnc_recurse_road_path",_this];
	_current_idx = (count _current_path);
	_min_distance = _current_idx * 0.7;
	_found_path = false;
	_return_path = [];
	_temp_path = [];
	_best_path = 1000000;
	_best_count = 0;
	if (_give_up > 0 && (time < _time_out)) then {
		_give_up = _give_up - 1;
		_roads = roadsConnectedTo _current_road;
		// diag_log Format["(%1)[%2]: %3 %4",diag_tickTime,"_fnc_recurse_road_path","start",[_current_idx, (count _roads), (count _current_path), _give_up, (_current_road distance _end_point), _total_dist]];
		while {(count _roads > 0) && (count _return_path <= 0)} do {
			_closest = 1000000;
			_road = objNull;
			{ 
				if ((_x distance _end_point) < _closest) then {
					_closest = (_x distance _end_point);
					_road = _x;
				};
			} foreach _roads;
			_roads set [ (_roads find _road), "XxDELxX"];
			_roads = _roads - ["XxDELxX"];
			_road_pos = getPos _road;
			_current_dist = _total_dist + (_road distance _current_road);
			if (_current_dist < (_end_distance * 2)) then {
				// Checking a position in an array of positions always returns false. So you have to use tricks...
				_road_serialized = format["%1_%2",(_road_pos select 0),(_road_pos select 1)];
				if ((!(_road in _current_path)) && (_best_count < 2)) then {
					if (((_road_pos distance _end_point) < _end_distance) && ((_road_pos distance (_current_path select 0)) > _min_distance)) then {
						_current_path set [count _current_path, _road];
						// [format["road_%1",_road_serialized], _road_pos, "ICON", "ColorBlack", [0.5,0.5],"",0,"mil_dot","",0.7] call XfCreateMarkerLocal;
						// We found a close enough road, this means we stop there.
						if ((_road_pos distance _end_point) < _max_distance) then {
							// diag_log Format["(%1)[%2]: %3 %4",diag_tickTime,"_fnc_recurse_road_path","CloseEnough",[_current_idx, (count _current_path), (_road_pos distance _end_point), _max_distance]];
							_best_count = _best_count + 1;
							_best_path = count _current_path;
							_return_path = _current_path
						} else {
							// We aren't close enough yet, so we continue searching
							_temp_path = [_road,_current_path,_end_point,_end_distance,_max_distance, _give_up, _current_dist, _time_out] call _fnc_recurse_road_path;
							if (count _temp_path <= 0) then {
								for [{_i=_current_idx},{_i < count _current_path},{_i = _i + 1}] do {
									_current_path set [_i, "xXdElXx"];
									_current_path = _current_path - ["xXdElXx"];
								};
								// diag_log Format["(%1)[%2]: %3",diag_tickTime,"_fnc_recurse_road_path",["DEAD END", _current_idx, (count _current_path)]];
							} else {
								_best_count = _best_count + 1;
								if (count _temp_path < _best_path) then {
									_best_path = count _temp_path;
									_return_path = _temp_path;
								};
							};
						};
					} else {
						// diag_log Format["(%1)[%2]: %3 %4",diag_tickTime,"_fnc_recurse_road_path","TooFarOrClose",[_current_idx, (count _current_path), (_road_pos distance _end_point), _end_distance]];
					};
				} else {
					// diag_log Format["(%1)[%2]: %3 %4",diag_tickTime,"_fnc_recurse_road_path","Exists",[_current_idx, _road_serialized]];
				};
			};
		};		
	};
	// diag_log Format["(%1)[%2]: %3 %4",diag_tickTime,"_fnc_recurse_road_path","end",[_current_idx, (count _roads), (count _return_path), _give_up]];
	_return_path;
};

_roads = _start nearRoads 50;
if (count _roads > 0) then {
	_nearest_road = objNull;
	_nearest_dist = 10000;
	{
		if ((_x distance _start) < _nearest_dist) then {
			_nearest_dist = (_x distance _start);
			_nearest_road = _x;
		};
	} foreach _roads;
	_path = [_nearest_road];
	_path = [_nearest_road,_path,_end,(_distance * 1.5),_max_dist, _give_up, 0,(time + _time_out)] call _fnc_recurse_road_path;
	// diag_log Format["(%1)[%2]: %3: %4",diag_tickTime,"XfGetRoadPath","Result", _path];
	if (count _path > 0) then {
		{	_deserialized set [ count _deserialized, getPos _x];
		} foreach _path;
	};
};
// diag_log Format["(%1)[%2]: %3: %4",diag_tickTime,"XfGetRoadPath","End", count _deserialized];

TRACE_1("End",count _deserialized);
TMFPROFILERSTOP;
_deserialized;